﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;

using Microsoft.Kinect;
using System.Speech.Synthesis;
using MySql.Data.MySqlClient;   //引用MySql
using System.Collections;   //导入动态数组ArrayList的命名空间
using System.Windows.Navigation; 
namespace BodyTrain
{
    /// <summary>
    /// LongJump.xaml 的交互逻辑
    /// </summary>
    public partial class LongJump : Page
    {
        #region 定义私有属性
        //Kinect相关变量
        KinectSensor kinectSensor = null;
        CoordinateMapper coordinateMapper = null;
        MultiSourceFrameReader reader;
        IList<Body> bodies;
        CameraMode _mode = CameraMode.Color;
        //姿势
        private Pose startPose; //开始姿势
        //private Pose endPose; //结束姿势
        //标志位
        bool start = false; //立定跳远Jump程序开始的标志位
        //语音
        bool soundStart = false; //检测的开始姿势，语音提示一次
        //bool soundEnd = false; //检测到结束姿势，语音提示一次
        static SpeechSynthesizer speaker1 = new SpeechSynthesizer();//语音定义
        //只处理一个Body数据
        ulong bodyId = 666; //初始化要跟踪目标的bodyId。不能初始化为0的原因是，当检测到一个人时，BodyCount为6，多余的5个body相应的Id为0。有效的Id是个17位的正整数
        //立定跳远
        ArrayList footLength = new ArrayList();
        float footLengthMean = 0; //估算的足长平均值
        ArrayList startFootY = new ArrayList();
        ArrayList startFootZ = new ArrayList();
        float startFootLocY = 0;
        float startFootLocZ = 0;
        bool footJump = false;  //脚部起跳标志位
        ArrayList startAnkleY = new ArrayList();
        ArrayList startAnkleZ = new ArrayList();
        float startAnkleLocY = 0;
        float startAnkleLocZ = 0;
        //bool ankleJump = false; //脚踝起跳标志位


        #endregion

        public LongJump()
        {
            InitializeComponent();//初始化窗口

            PopulatePoseLibrary();
        }
        #region 姿势判断相关函数
        private void PopulatePoseLibrary()//姿势库
        {
            //开始测距 Pose - 伸开双臂
            this.startPose = new Pose();
            this.startPose.Title = "伸开双臂(Start Messure)";
            this.startPose.Angles = new PoseAngle[4];
            this.startPose.Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20);
            this.startPose.Angles[1] = new PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 180, 20);
            this.startPose.Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20);
            this.startPose.Angles[3] = new PoseAngle(JointType.ElbowRight, JointType.WristRight, 0, 20);

            ////结束测量姿势 -举起手来 Both Hands Up
            //this.endPose = new Pose();
            //this.endPose.Title = "双手前臂举起(Forearms Up)";
            //this.endPose.Angles = new PoseAngle[4];
            //this.endPose.Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20);
            //this.endPose.Angles[1] = new PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 90, 20);
            //this.endPose.Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20);
            //this.endPose.Angles[3] = new PoseAngle(JointType.ElbowRight, JointType.WristRight, 90, 20);

            ////切换界面姿势-双手向前平举，再配合骨骼点判断手臂前伸
            //this.switchPose = new Pose();
            //this.switchPose.Title = "双手向前平举(Straighten Arms Forward)";
            //this.switchPose.Angles = new PoseAngle[4];
            //this.switchPose.Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20);
            //this.switchPose.Angles[1] = new PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 180, 20);
            //this.switchPose.Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20);
            //this.switchPose.Angles[3] = new PoseAngle(JointType.ElbowRight, JointType.WristRight, 90, 20);
        }
        private void processGesture(Body body, Dictionary<JointType, Point> jointPoints)//姿势匹配
        {
            if (IsPose(body, this.startPose, jointPoints))      //检测到开始姿势，则开始
            {
                start = true;
                remind1.Text = "测量开始";

                if (!soundStart) //仅播放一次声音
                {
                    speaker1.SpeakAsync("测量开始");
                    soundStart = true; //已经播放一次声音了
                    //soundEnd = false; //可以播放测量结束语音提示
                }

                //记录开始姿势时，起始位置的数据
                var footLeft = body.Joints[JointType.FootLeft].Position; //左脚位置
                var footRight = body.Joints[JointType.FootRight].Position; //右脚位置 
                var footFar = (footLeft.Z < footRight.Z) ? footLeft : footRight; // 比较左右脚哪个小 把 其中小的值给footFar
                startFootY.Add(footFar.Y);
                startFootZ.Add(footFar.Z);

                var ankleLeft = body.Joints[JointType.AnkleLeft].Position; //左脚踝位置
                var ankleRight = body.Joints[JointType.AnkleRight].Position; //右脚踝位置 
                var ankleFar = (ankleLeft.Z < ankleRight.Z) ? ankleLeft : ankleRight; // 比较左右脚哪个小 把 其中小的值给ankleFar
                startAnkleY.Add(ankleFar.Y);
                startAnkleZ.Add(ankleFar.Z);

                var tempFootLength = body.Height() / 6.876 + 0.025;
                footLength.Add(tempFootLength);

                /////////////////////////////////        用于指定校正曲线        //////////////////////////////////////////////////////////
                //Joint ankleLeft = body.Joints[JointType.AnkleLeft];
                //Joint footLeft = body.Joints[JointType.FootLeft];
                //Joint ankleRight = body.Joints[JointType.AnkleRight];
                //Joint footRight = body.Joints[JointType.FootRight];

                //Console.WriteLine("{0} {1} {2} {3} {4} {5} {6} {7}", ankleLeft.Position.Y, ankleLeft.Position.Z, footLeft.Position.Y, footLeft.Position.Z, ankleRight.Position.Y, ankleRight.Position.Z, footRight.Position.Y, footRight.Position.Z);

            }

            //if (IsPose(body, endPose, jointPoints) && (body.Joints[JointType.WristLeft].Position.Y > body.Joints[JointType.ShoulderLeft].Position.Y)) //检测到结束姿势，测量结束
            //{
            //    start = false;
            //    remind1.Text = "测量结束";
            //    if (!soundEnd) //仅播放一次声音
            //    {
            //        speaker1.SpeakAsync("测量结束");
            //        soundEnd = true; //已经播放一次声音了
            //        soundStart = false; //可以播放测量开始语音提示
            //    }
            //}

        }
        //判断是否和指定的姿势匹配
        private bool IsPose(Body body, Pose pose, IDictionary<JointType, Point> jointPoints)
        {
            bool isPose = true;
            double angle;
            double poseAngle;
            double poseThreshold;
            double loAngle;
            double hiAngle;

            for (int i = 0; i < pose.Angles.Length && isPose; i++)
            {
                poseAngle = pose.Angles[i].Angle;
                poseThreshold = pose.Angles[i].Threshold;
                angle = GetJointAngle(jointPoints, pose.Angles[i].CenterJoint, pose.Angles[i].AngleJoint);

                hiAngle = poseAngle + poseThreshold;
                loAngle = poseAngle - poseThreshold;

                if (hiAngle >= 360 || loAngle < 0)
                {
                    loAngle = (loAngle < 0) ? 360 + loAngle : loAngle;
                    hiAngle = hiAngle % 360;

                    isPose = !(loAngle > angle && angle > hiAngle);
                }
                else
                {
                    isPose = (loAngle <= angle && hiAngle >= angle);
                }
            }
            return isPose;
        }

        //获取骨骼点角度
        private double GetJointAngle(IDictionary<JointType, Point> jointPoints, JointType centerJoint, JointType angleJoint)
        {
            Point primaryPoint = jointPoints[centerJoint];
            Point anglePoint = jointPoints[angleJoint];
            Point x = new Point(primaryPoint.X + anglePoint.X, primaryPoint.Y);

            double a;
            double b;
            double c;

            a = Math.Sqrt(Math.Pow(primaryPoint.X - anglePoint.X, 2) + Math.Pow(primaryPoint.Y - anglePoint.Y, 2));
            b = anglePoint.X;
            c = Math.Sqrt(Math.Pow(anglePoint.X - x.X, 2) + Math.Pow(anglePoint.Y - x.Y, 2));

            double angleRad = Math.Acos((a * a + b * b - c * c) / (2 * a * b));
            double angleDeg = angleRad * 180 / Math.PI;

            if (primaryPoint.Y < anglePoint.Y)
            {
                angleDeg = 360 - angleDeg;
            }
            return angleDeg;
        }

        #endregion

        #region Kinect相关函数
        //相机模式，彩色、深度、红外
        private enum CameraMode
        {
            Color,
            Depth,
            Infrared
        }

        private void Page_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensor = KinectSensor.GetDefault();
            coordinateMapper = kinectSensor.CoordinateMapper;
            if (kinectSensor != null)
            {
                kinectSensor.Open();
                this.reader = kinectSensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color | FrameSourceTypes.Depth | FrameSourceTypes.Infrared | FrameSourceTypes.Body);
                this.reader.MultiSourceFrameArrived += reader_MultiSourceFrameArrived;
            }
            //语音设置
            speaker1.SelectVoiceByHints(VoiceGender.Female, VoiceAge.Adult, 2, System.Globalization.CultureInfo.CurrentCulture);
            speaker1.Rate = -2;
            speaker1.Volume = 100;

            speaker1.SpeakAsync("立定跳远");
        }

        private void Page_Closed(object sender, RoutedEventArgs e)
        {
            //程序结束，数据上传
            String user = Application.Current.Properties["userName"].ToString();
            String relation = Application.Current.Properties["relation"].ToString();
            //数据库操作
            MySQL_Service mySQL = new MySQL_Service();
            mySQL.ConnectSer();
            mySQL.InsertChildren(user, relation, "LongJump", 2);
            mySQL.CloseServer();


            //释放资源？
            if (this.reader != null)
            {
                // ColorFrameReder is IDisposable接口
                this.reader.Dispose();
                this.reader = null;
            }
            //关闭Kinect传感器
            if (this.kinectSensor != null)
            {
                this.kinectSensor.Close();
                this.kinectSensor = null;
            }
        }

        void reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference.AcquireFrame();
            //彩色
            using (var frame = reference.ColorFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Color)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }
            // Body
            using (var frame = reference.BodyFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    canvas.Children.Clear();

                    bodies = new Body[frame.BodyFrameSource.BodyCount]; //获取当前帧所有人骨骼点数据

                    frame.GetAndRefreshBodyData(bodies); //获取刷新的身体数据

                    TrackOne(bodies, frame); //获取满足条件的BodyId

                    foreach (var body in bodies) //当捕获到多人骨骼点时，一个个遍历
                    {
                        //if (body.IsTracked)     //骨骼点被跟踪
                        if (body.IsTracked && (bodyId == body.TrackingId)) //骨骼点被跟踪，且相应的Id是跟踪目标
                        {

                            canvas.DrawSkeleton(body, coordinateMapper);//在canvas中绘制骨骼

                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
                            Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
                            foreach (JointType jointType in body.Joints.Keys)
                            {
                                jointPoints[jointType] = joints[jointType].ScaleTo(coordinateMapper);
                            }
                            processGesture(body, jointPoints);      //匹配姿势

                            Jump(body); //立定跳远测量程序
                        }
                    }
                }
            }
        }

        #endregion     

        //获取BodyId，便于之后实现只跟踪一个人的目标 
        private void TrackOne(IList<Body> bodies, BodyFrame frame)  //只处理一人数据
        {
            //这个Id选取站立在图像中心人的bodyId。当丢失这个Id时，再次获取图像中心人的bodyId。
            //判断人是否在于中心，是利用SpineMid骨骼点的水平坐标值(X轴坐标)，判断其绝对值是否小于0.16。
            //kinect中心是X轴的原点

            int bodyNum = frame.BodyFrameSource.BodyCount; //获得图像帧中身体数目
            ArrayList bodyIds = new ArrayList(); //存放图像帧中bodyId
            int i = 0;
            float temp = 666; //临时存放水平坐标值 

            for (i = 0; i < bodyNum; i++) //记录bodyId
            {
                bodyIds.Add(bodies[i].TrackingId); //存放bodyId
            }

            if (bodyIds.Contains(bodyId)) //判断bodyId是否在该图像帧
            {

            }
            else //不在图像帧中
            {
                for (i = 0; i < bodyNum; i++)
                {
                    if (bodies[i].TrackingId > 666) //剔除无效的Id
                    {
                        temp = bodies[i].Joints[JointType.SpineMid].Position.X;

                        if ((Math.Abs(temp) < 0.16f) && bodies[i].IsTracked) //水平坐标绝对值小于阈值，且相应的身体被追踪
                        {
                            bodyId = bodies[i].TrackingId;
                            break;
                        }
                    }
                }
            }
        }

        //立定跳远测量程序
        private void Jump(Body body)
        {
            if (start)
            {
                int discardNum = 5; //丢弃最大值的数量，和最小值的数量
                int i = discardNum;
                if (startFootY.Count > 25)
                {
                    ArrayList tempStartFootY = new ArrayList(startFootY);
                    ArrayList tempStartFootZ = new ArrayList(startFootZ);
                    float tempStartFootLocY = 0;
                    float tempStartFootLocZ = 0;

                    tempStartFootY.Sort();
                    tempStartFootZ.Sort();

                    for (i = discardNum; i < (startFootY.Count - discardNum); i++)
                    {
                        tempStartFootLocY += (float)tempStartFootY[i];
                        tempStartFootLocZ += (float)tempStartFootZ[i];
                    }

                    startFootLocY = tempStartFootLocY / (startFootY.Count - discardNum * 2);
                    startFootLocZ = tempStartFootLocZ / (startFootY.Count - discardNum * 2);
                }

                if (startAnkleY.Count > 25)
                {
                    ArrayList tempStartAnkleY = new ArrayList(startAnkleY);
                    ArrayList tempStartAnkleZ = new ArrayList(startAnkleZ);

                    float tempStartAnkleLocY = 0;
                    float tempStartAnkleLocZ = 0;

                    tempStartAnkleY.Sort();
                    tempStartAnkleZ.Sort();

                    for (i = discardNum; i < (startAnkleY.Count - discardNum); i++)
                    {
                        tempStartAnkleLocY += (float)tempStartAnkleY[i];
                        tempStartAnkleLocZ += (float)tempStartAnkleZ[i];
                    }

                    startAnkleLocY = tempStartAnkleLocY / (startAnkleY.Count - discardNum * 2);
                    startAnkleLocZ = tempStartAnkleLocZ / (startAnkleY.Count - discardNum * 2);
                }

                if (footLength.Count > 25)
                {
                    ArrayList tempFootLength = new ArrayList(footLength);
                    double footLengthSum = 0;

                    tempFootLength.Sort();

                    for (i = discardNum; i < (footLength.Count - discardNum); i++)
                    {
                        footLengthSum += (double)tempFootLength[i];
                    }
                    footLengthMean = (float)footLengthSum / (footLength.Count - discardNum * 2);
                }

                if ((startFootLocY != 0.0f) && (startAnkleLocY != 0.0f))
                {
                    var footLeft = body.Joints[JointType.FootLeft].Position; //左脚位置
                    var footRight = body.Joints[JointType.FootRight].Position; //右脚位置 
                    var footFar = (footLeft.Z > footRight.Z) ? footLeft : footRight; // 比较左右脚哪个大 把 其中大的值给footFar
                    float nowFootLocY = (float)0.0262 * footFar.Z - (float)0.4622; //通过校正公式矫正后，当前的Y值

                    //var ankleLeft = body.Joints[JointType.AnkleLeft].Position; //左脚踝位置
                    //var ankleRight = body.Joints[JointType.AnkleRight].Position; //右脚位置 
                    //var ankleFar = (ankleLeft.Z > ankleRight.Z) ? ankleLeft : ankleRight; // 比较左右脚哪个大 把 其中大的值给ankleFar
                    //float nowAnkleLocY = (float)0.03318 * ankleFar.Z - (float)0.4552; //通过校正公式矫正后，当前的Y值

                    if ((footFar.Y - startFootLocY) > 0.10f)
                    {
                        footJump = true;

                        //Console.WriteLine("脚部跳起");
                    }
                    else
                    {
                        Console.WriteLine("脚部跳起高度差  {0}", footFar.Y - startFootLocY);
                    }

                    if (footJump && (Math.Abs(nowFootLocY - startFootLocY) < 0.04f))
                    {
                        float footJumpDist = startFootLocZ - footFar.Z - footLengthMean;
                        jumpValue.Text = footJumpDist.ToString();
                    }
                    if (footJump)
                    {
                        Console.WriteLine("脚部高度差值  {0}", Math.Abs(nowFootLocY - startFootLocY));
                    }


                    //if ((ankleFar.Y - startAnkleLocY) > 0.10f)
                    //{
                    //    ankleJump = true;

                    //    //Console.WriteLine("脚踝跳起");
                    //}
                    //else
                    //{
                    //    Console.WriteLine("脚踝跳起高度差  {0}", ankleFar.Y - startAnkleLocY);
                    //}

                    //if (ankleJump && (Math.Abs(nowAnkleLocY - startAnkleLocY) < 0.04f))
                    //{
                    //    float ankleJumpDist = startAnkleLocZ - ankleFar.Z - footLengthMean;
                    //    jumpValue2.Text = ankleJumpDist.ToString();
                    //}
                    //if (ankleJump)
                    //{
                    //    Console.WriteLine("脚踝高度差值  {0}", Math.Abs(nowAnkleLocY - startAnkleLocY));
                    //}
                }
                

            }
        }

        private void GoBack_Click(object sender, RoutedEventArgs e)
        {
            NavigationService.GetNavigationService(this).Navigate(new Uri("/ChildrenTrain/ChildrenTrain.xaml", UriKind.Relative));
        }

    }
}
